//
// Created by liz on 24-12-7.
//

#include "command.h"
#include "chassis.h"
#include "arm_cmd.h"

#include "bsp_usart.h"

#include <cmsis_os2.h>
#include <controller.h>


#include "stdio.h"
#include <ctype.h>
#include <general_def.h>
#include <math.h>
#include <message.h>
#include <message_center.h>
#include <remote_control.h>

#include "mastermachine.h"
#include "robot.h"

#include "string.h"
#include "ins_task.h"

#include "QRcode.h"


static Publisher_t *chassis_cmd_pub;   // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者

static Chassis_Ctrl_Cmd_s chassis_cmd_send;
static Chassis_Feedback_s chassis_feed_recv;

static RC_ctrl_t *rc_ctrl;

static PIDInstance pid_calibration_x,pid_calibration_y;

static Master_send_s master_send;
static Master_recv_s *master_recv;

static QRCodeInstance *qrcode;
int16_t tasks[10];
uint16_t task_count;

static uint8_t back=0;

static void QR_Wait();



void extract_numbers(const char *input, int16_t tasks[], uint16_t *task_count) ;
void Move_Calibrate(State_e state);
void Move_NoCalibrate();
void Move_Detect(bool wait);
void enter_state(State_e state);



void Command_Init()
{
    rc_ctrl = RemoteControlInit(&huart5);
    QRCode_Init_Config_s qrcode_config={
        .usart_handle = &huart4,
    };
    qrcode = QRCode_Init(&qrcode_config);

    PID_Init_Config_s pid_init_config={
        .Type = PID_TYPE_INCREMENT,
        .Kp = 0.00999999791, // 4.5
        .Ki = 0.000300000012,  // 0
        .Kd = 0.000072,  // 0
        .DeadBand = 0,
        .IntegralLimit = 0.03,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement ,
        .MaxOut = 0.2,
        .MaxDelta = 0.007,
    };
    PIDInit(&pid_calibration_x,&pid_init_config);
    pid_init_config.MaxOut = 0.20;
    PIDInit(&pid_calibration_y,&pid_init_config);

    chassis_cmd_pub = PubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s));
    chassis_feed_sub = SubRegister("chassis_feed",sizeof(Chassis_Feedback_s));


}


uint8_t start_target[] = {0x55, 0x01, 0x01};
uint8_t text0[30]={0};
uint8_t *text0_p=text0;
uint8_t text1[50];
void Command_Execute()
{
    //HAL_UART_Transmit(&huart1,"Hello",strlen("Hello"),5);
    master_recv=CommunicateInit(&huart1);
    // //debug则关掉这个
    // INS_Start();



    while(HAL_GPIO_ReadPin(START_KEY_GPIO_Port,START_KEY_Pin)==GPIO_PIN_SET)
    {
        osDelay(10);
    }
    // IMU_SetOffset();

    osDelay(500);

    action_init();
    sprintf(text1,"page Number\xff\xff\xff");         //注意串口屏全局变量和局部变量
    HAL_UART_Transmit_DMA(&huart5,text1,strlen(text1));
    osDelay(5);


    sprintf(text1,"Number.tm1.en=1\xff\xff\xff"); //局部变量不能在其他页面内修改
    HAL_UART_Transmit_DMA(&huart5,text1,strlen(text1));
    osDelay(5);

     /**********************
     sprintf(text1,"0\xff\xff\xff");
     HAL_UART_Transmit(&huart5,text1,strlen(text1),5);
     osDelay(50);
     sprintf(text1,"page Key\xff\xff\xff");
     HAL_UART_Transmit(&huart5,text1,strlen(text1),5);
     osDelay(200);

     while(strstr(text0_p,start_target)==NULL)
     {
         static uint32_t count;
         count++;
         //HAL_UART_Transmit(&huart1,text0,strlen(text0),5);
         if(text0_p==0&&text0_p[1]!=0)
         {
             if(text0_p!=&text0[29])
                 text0_p++;
             else
                 text0_p=text0;
         }
         HAL_UART_AbortReceive(&huart5);
         HAL_UART_Receive_DMA(&huart5,text0,10);
         osDelay(100);
     }
    **********************/
    HAL_UART_AbortReceive(&huart5);
    Chassis_Start();
    Move_SetMaxSpeed(0.3);
    // Move_SetMaxSpeed(0);
    osDelay(100);

    // enter_state(STATE_PICK_RAW);
    // Move_Calibrate(STATE_PICK_RAW);
    // // debug_wait();

    enter_state(STATE_IDLE);
    // //开始了
    Move_Target(0.2,0.2,0,true);      //斜着走方法

    // Move_Target(0,0.23,0,true);      //先横着出去方法
    osDelay(200);
    //去抓取，顺便扫码
    Move_Target(1.45,0.22,0,false);
    QR_Wait();



    //尝试到达抓取位置
    //此处就提速，这样就可以节省时间
    Move_SetMaxSpeed(0.5);
    Move_Target(1.45,0.22,0,true);
    Move_Target(1.45,0.15,0,true);


    enter_state(STATE_PICK_RAW);
    action_calibrate(STATE_PICK_RAW);
    Move_SetMaxSpeed(0.15);
    Move_Calibrate(STATE_PICK_RAW);

    action_pick_raw(tasks[0]);

    Move_SetMaxSpeed(0.8);
    Move_NoCalibrate();
    //
    // 开始前往粗加工
    action_calibrate(STATE_IDLE);
    Move_Target(1.05,0.15,0,true);
    Move_Target(1.05,0.15,90,true);
    Move_Target(1.05,1.850,90,true);
    Move_Target(1.05,1.850,180,true);


    enter_state(STATE_ROUGH_AREA);
    action_calibrate(STATE_ROUGH_AREA);
    Move_SetMaxSpeed(0.15);

    // debug_wait();

    Move_Calibrate(STATE_ROUGH_AREA);
    // debug_wait();
    action_put_rough(tasks[0]);
    action_take_rough(tasks[0]);

    Move_SetMaxSpeed(0.7);
    Move_NoCalibrate();



    // debug_wait();
    // //
    // //

    //前往精加工
    action_calibrate(STATE_IDLE);
    // osDelay(1000);
    Move_Target(1.85,1.85,180,true);
    Move_Target(1.85,1.85,90,true);             //注释此条可边转边走
    Move_Target(1.85,1.06,90,true);
    // osDelay(1000);

    enter_state(STATE_ROUGH_AREA);
    action_calibrate(STATE_ROUGH_AREA);
    Move_SetMaxSpeed(0.15);

    Move_Calibrate(STATE_ROUGH_AREA);
    action_put_rough(tasks[0]);     //精加工放下

    Move_SetMaxSpeed(0.7);
    Move_NoCalibrate();

    action_calibrate(STATE_IDLE);
    Move_Target(1.85,0.15,90,true);
    Move_Target(1.85,0.15,0,true);
    Move_Target(1.45,0.15,0,true);

    EmptyMasterTimer();

    enter_state(STATE_PICK_RAW);
    action_calibrate(STATE_PICK_RAW);
    Move_SetMaxSpeed(0.15);
    Move_Calibrate(STATE_PICK_RAW);

    action_pick_raw(tasks[1]);

    Move_SetMaxSpeed(0.8);
    Move_NoCalibrate();

    //
    // 开始前往粗加工
    Move_Target(1.03,0.15,0,true);
    Move_Target(1.03,0.15,90,true);
    Move_Target(1.03,1.850,90,true);
    Move_Target(1.03,1.850,180,true);


    enter_state(STATE_ROUGH_AREA);
    action_calibrate(STATE_ROUGH_AREA);
    Move_SetMaxSpeed(0.15);

    // debug_wait();

    Move_Calibrate(STATE_ROUGH_AREA);
    // debug_wait();
    action_put_rough(tasks[1]);
    action_take_rough(tasks[1]);

    Move_SetMaxSpeed(0.7);
    Move_NoCalibrate();



    // debug_wait();
    // //
    // //

    //前往精加工
    // osDelay(1000);
    Move_Target(1.85,1.85,180,true);
    Move_Target(1.85,1.85,90,true);             //注释此条可边转边走
    Move_Target(1.85,1.06,90,true);


    enter_state(STATE_FASHIONING_AREA);
    action_calibrate(STATE_FASHIONING_AREA);
    Move_SetMaxSpeed(0.15);

    Move_Calibrate(STATE_FASHIONING_AREA);
    // debug_wait();

    action_put_fashioning(tasks[1]);

    Move_SetMaxSpeed(0.5);
    Move_NoCalibrate();

    // 回家代码
    // while(!back)
    //     osDelay(1);
    Move_NoCalibrate();
    osDelay(500);
    Move_Target(chassis_cmd_send.target_x,chassis_cmd_send.target_y,0,true);
    Move_Target(0.23,chassis_cmd_send.target_y,0,true);
    Move_Target(0.23,0.23,0,true);
    // Move_Target(0,chassis_cmd_send.target_y,90,true);
    // osDelay(1000);
    // Move_Target(0.0,0,90,true);
    Move_Target(0.0,0,0,true);

}

void Command_Test()
{
    if(master_recv->move_to!=0)
    {
        uint8_t moveto = master_recv->move_to;
        master_recv->move_to=0;
        switch (moveto)
        {
            case 1:
                Move_Relative(1,0,0,true);
                break;
            case 2:
                Move_Relative(-1,0,0,true);
                break;
            case 3:
                Move_Relative(0,1,0,true);
                break;
            case 4:
                Move_Relative(0,-1,0,true);
                break;
            case 5:
                Move_Relative(0,0,90,true);
                break;
            case 6:
                Move_Relative(0,0,-90,true);
                break;
            default:
                break;

        }
    }
}

void QR_Wait()
{
    while(qrcode->readable!=1) {
        static uint32_t qr_waittime;
        osDelay(2);
        qr_waittime+=2;
        if(qr_waittime>=5000)
        {
            qr_waittime=0;
            Move_Target(0.22,0.22,0,true);
        }
    }

    char *QRResult=(char *)QRCode_ReadBuffer(qrcode);
    extract_numbers(QRResult,tasks,&task_count);


    sprintf(text1,"Number.t0.txt=\"%.*s\"\xff\xff\xff",7,QRResult); //局部变量不能在其他页面内修改
    HAL_UART_Transmit_DMA(&huart5,text1,strlen(text1));
    osDelay(5);

    sprintf(text1,"Number.tm0.en=0\xff\xff\xff"); //局部变量不能在其他页面内修改
    HAL_UART_Transmit_DMA(&huart5,text1,strlen(text1));
    osDelay(5);

    sprintf(text1,"Number.t0.pco=0\xff\xff\xff"); //局部变量不能在其他页面内修改
    HAL_UART_Transmit_DMA(&huart5,text1,strlen(text1));
    osDelay(5);



}

/**
 * @brief 移动到位检测，如果wait为true则执行。
 * @param wait 是否进行检测
 * @note
 */
void Move_Detect(bool wait)
{
    while(wait) {
        static uint32_t stuck_count;
        SubGetMessage(chassis_feed_sub,&chassis_feed_recv);
        float delta_x = chassis_feed_recv.current_x-(chassis_cmd_send.target_x+chassis_cmd_send.offset_x);
        float delta_y = chassis_feed_recv.current_y-(chassis_cmd_send.target_y+chassis_cmd_send.offset_y);
        float delta_yaw = chassis_feed_recv.current_yaw-chassis_cmd_send.target_yaw;
        if(fabsf(delta_x)>0.05f||
            fabsf(delta_y)>0.05f||
            fabsf(delta_yaw)>1.0f)
        {
            stuck_count++;
            osDelay(2);
            //此处假设所有移动都不会超过10s
            if(stuck_count>=5000)
            {
                stuck_count=0;
                break;

            }
        }else {
            stuck_count=0;
            break;
        }
    }
}

void Move_Relative(float x,float y,float yaw,bool wait)
{
    chassis_cmd_send.target_x+=x;
    chassis_cmd_send.target_y+=y;
    chassis_cmd_send.target_yaw+=yaw;
    PubPushMessage(chassis_cmd_pub,&chassis_cmd_send);
    Move_Detect(wait);
}

void Move_Target(float target_x,float target_y,float target_yaw,bool wait)
{
    chassis_cmd_send.target_x=target_x;
    chassis_cmd_send.target_y=target_y;
    chassis_cmd_send.target_yaw=target_yaw;
    PubPushMessage(chassis_cmd_pub,&chassis_cmd_send);
    Move_Detect(wait);
}

void Move_SetMaxSpeed(float speed)
{
    chassis_cmd_send.max_speed=speed;
    PubPushMessage(chassis_cmd_pub,&chassis_cmd_send);
}


void Move_Calibrate(State_e state)
{
    static float delta_distance = 0.0f;
    static uint16_t correct_cnt;            //校准到容许范围的计数器
    static uint32_t cali_cnt=0;
    
    correct_cnt=0;
    cali_cnt=0;
    delta_distance = sqrt(pow(master_recv->xdis,2) + pow(master_recv->ydis,2));
    pid_calibration_x.Last_Output=0;
    pid_calibration_x.Last_Err=0;
    pid_calibration_y.Last_Output=0;
    pid_calibration_y.Last_Err=0;
    // while(delta_distance>=50)
    while(1)
    {
        static uint16_t send_cnt=0;
        if(send_cnt++>=50)
        {
            send_cnt=0;
            //重复当前状态的发送函数
            enter_state(state);
        }

        // 假设IMU Yaw绝对正确，那么就可以通过Yaw来获取摄像头xy和世界坐标xy的关系
        static float sin_theta, cos_theta;
        static float x_distance_world, y_distance_world;        //世界坐标系的xy距离

        //计算夹角的sin和cos值
        arm_sin_cos_f32(chassis_feed_recv.current_yaw,&sin_theta,&cos_theta);


        //master_recv->xdis和ydis是车体坐标系。严格来说，还可以通过机械臂yaw来进一步转换角度。
        x_distance_world = master_recv->xdis*cos_theta - master_recv->ydis*sin_theta;
        y_distance_world = master_recv->xdis*sin_theta + master_recv->ydis*cos_theta;

        if(state == STATE_PICK_RAW)
        {
            if(master_recv->is_stop==1)
            {
                PIDCalculate(&pid_calibration_x,x_distance_world,0);
                PIDCalculate(&pid_calibration_y,y_distance_world,0);
            }
            else
            {
                // PIDCalculate(&pid_calibration_x,x_distance_world,master_recv->xdis);
                // PIDCalculate(&pid_calibration_y,y_distance_world,master_recv->ydis);
            }
        }
        else
        {
            PIDCalculate(&pid_calibration_x,x_distance_world,0);
            PIDCalculate(&pid_calibration_y,y_distance_world,0);
        }


        chassis_cmd_send.offset_x = pid_calibration_x.Output;
        chassis_cmd_send.offset_y = pid_calibration_y.Output;

        PubPushMessage(chassis_cmd_pub,&chassis_cmd_send);
        osDelay(10);

        if(state == STATE_PICK_RAW)
        {
            if(fabsf(x_distance_world)<=30 && fabsf(y_distance_world)<=40 )
                correct_cnt++;
            else
            {
                correct_cnt=0;
            }
            if(correct_cnt >=100)
            {
                // Move_SetMaxSpeed(0);
                break;
            }
        }
        else
        {

            cali_cnt++;

            if(fabsf(x_distance_world)<=20 && fabsf(y_distance_world)<=20)
                correct_cnt++;
            else
            {
                correct_cnt=0;
                // Move_SetMaxSpeed(0.12);
            }
            if(correct_cnt >=60||cali_cnt>=1200)
            {
                correct_cnt=60;
                // Move_SetMaxSpeed(0);
                break;
            }
        }


    }


}

void Move_NoCalibrate()
{
    chassis_cmd_send.offset_x = 0;
    chassis_cmd_send.offset_y = 0;
    PubPushMessage(chassis_cmd_pub,&chassis_cmd_send);
}

void extract_numbers(const char *input, int16_t tasks[], uint16_t *task_count) {
    int num = 0;
    *task_count = 0;  // 初始化任务计数器

    while (*input) {
        if (isdigit(*input)) {
            num = 0;  // 清空当前数字
            // 提取当前数字
            while (*input && isdigit(*input)) {
                num = num * 10 + (*input - '0');
                input++;
            }
            // 将数字存储到 tasks 数组
            if (*task_count < 10) {  // 确保不超过数组大小
                tasks[*task_count] = num;
                (*task_count)++;  // 增加计数
            }
        } else {
            input++;  // 跳过非数字字符
        }
    }
}

void enter_state(State_e state)
{
    master_send.state = state;
    CommunicateSend(&master_send);
}
